﻿using System;
using System.IO;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Xml;
using System.Xml.Serialization;
using SharpKml.Base;
using SharpKml.Dom;

namespace Geotagging1
{
    
    public partial class Geotagger : Form
    {
        private List<string> fileList = new List<string>();
        private List<string> picList = new List<string>();
        private List<DateTime> list = new List<DateTime>();
        public string jpgpath;
        public string gpx_logfile;
        public string[] getTime;
        string height;
        DateTime isTime;
        DateTime Date1;
        string[] gpsTimes;
        string[] PicDates;
        DateTime[] PictureDates;
        DateTime earliest_picture;
        double eTime;
        int i = 0;
        const float rad2deg = (float)(180 / Math.PI);
        const float deg2rad = (float)(1.0 / rad2deg);
        
        public class Rational
        {
            uint dem = 0;
            uint num = 0;

            public Rational(double input)
            {
                Value = input;
            }

            public byte[] GetBytes()
            {
                byte[] answer = new byte[8];

                Array.Copy(BitConverter.GetBytes((uint)num), 0, answer, 0, sizeof(uint));
                Array.Copy(BitConverter.GetBytes((uint)dem), 0, answer, 4, sizeof(uint));

                return answer;
            }

            public double Value
            {
                get
                {
                    return num / dem;
                }
                set
                {
                    if ((value % 1.0) != 0)
                    {
                        dem = 100; num = (uint)(value * dem);
                    }
                    else
                    {
                        dem = 1; num = (uint)(value);
                    }
                }
            }
        }
        public Geotagger()
        {
            InitializeComponent();
        }

        private void DoImageBrowse(object sender, EventArgs e)
        {
            DialogResult result = folderBrowserDialog1.ShowDialog();
            if (result == DialogResult.OK)
            {
                jpgpath = folderBrowserDialog1.SelectedPath;
                textBox1.Text = folderBrowserDialog1.SelectedPath;
            }
        }
        private void DoLogBrowse(object sender, EventArgs e)
        {
            DialogResult result = openFileDialog1.ShowDialog();
            if (result == DialogResult.OK)
            {
                gpx_logfile = openFileDialog1.FileName;
                textBox2.Text = openFileDialog1.FileName;
            }
        }
        private double ConvertToTimestamp(DateTime value)
        {
            System.TimeSpan span = (value - new DateTime(1970, 1, 1, 0, 0, 0, 0).ToLocalTime());
            return (double)span.TotalSeconds;
        }
        static DateTime ConvertFromUnixTimestamp(double timestamp)
        {
            DateTime origin = new DateTime(1970, 1, 1, 0, 0, 0, 0);
            return origin.AddSeconds(timestamp);
        }
        public static double ToDouble(string In)
        {
            In = In.Replace(",", ".");
            return double.Parse(In, System.Globalization.CultureInfo.InvariantCulture);
        }
        
        byte[] coordtobytearray(double coordin)
        {
            double coord = Math.Abs(coordin);

            byte[] output = new byte[sizeof(double) * 3];

            int d = (int)coord;
            int m = (int)((coord - d) * 60);
            double s = ((((coord - d) * 60) - m) * 60);
            /*
21 00 00 00 01 00 00 00--> 33/1
18 00 00 00 01 00 00 00--> 24/1
06 02 00 00 0A 00 00 00--> 518/10
*/

            Array.Copy(BitConverter.GetBytes((uint)d), 0, output, 0, sizeof(uint));
            Array.Copy(BitConverter.GetBytes((uint)1), 0, output, 4, sizeof(uint));
            Array.Copy(BitConverter.GetBytes((uint)m), 0, output, 8, sizeof(uint));
            Array.Copy(BitConverter.GetBytes((uint)1), 0, output, 12, sizeof(uint));
            Array.Copy(BitConverter.GetBytes((uint)(s * 10)), 0, output, 16, sizeof(uint));
            Array.Copy(BitConverter.GetBytes((uint)10), 0, output, 20, sizeof(uint));

            return output;
        }


        
        void WriteCoordinatesToImage(string Filename, double dLat, double dLong, double alt)
        {
            using (MemoryStream ms = new MemoryStream(File.ReadAllBytes(Filename)))
            {
                Application.DoEvents();

                using (Image Pic = Image.FromStream(ms))
                {
                    PropertyItem[] pi = Pic.PropertyItems;

                    pi[0].Id = 0x0004;
                    pi[0].Type = 5;
                    pi[0].Len = sizeof(ulong) * 3;
                    pi[0].Value = coordtobytearray(dLong);
                    Pic.SetPropertyItem(pi[0]);

                    pi[0].Id = 0x0002;
                    pi[0].Type = 5;
                    pi[0].Len = sizeof(ulong) * 3;
                    pi[0].Value = coordtobytearray(dLat);
                    Pic.SetPropertyItem(pi[0]);

                    pi[0].Id = 0x0006;
                    pi[0].Type = 5;
                    pi[0].Len = 8;
                    pi[0].Value = new Rational(alt).GetBytes();
                    Pic.SetPropertyItem(pi[0]);

                    pi[0].Id = 1;
                    pi[0].Len = 2;
                    pi[0].Type = 2;
                    if (dLat < 0)
                    {
                        pi[0].Value = new byte[] { (byte)'S', 0 };
                    }
                    else
                    {
                        pi[0].Value = new byte[] { (byte)'N', 0 };
                    }
                    Pic.SetPropertyItem(pi[0]);

                    pi[0].Id = 3;
                    pi[0].Len = 2;
                    pi[0].Type = 2;
                    if (dLong < 0)
                    {
                        pi[0].Value = new byte[] { (byte)'W', 0 };
                    }
                    else
                    {
                        pi[0].Value = new byte[] { (byte)'E', 0 };
                    }
                    Pic.SetPropertyItem(pi[0]);
                    string outputfilename = Path.GetDirectoryName(Filename).ToLower() + Path.DirectorySeparatorChar + "gtg_" + Path.GetFileName(Filename).ToLower();
                    File.Delete(outputfilename);
                    Pic.Save(outputfilename);
                }
            }
        }

        private void GeotagImages(object sender, EventArgs e)
        {
            richTextBox1.Text = "";
            if (string.IsNullOrEmpty(jpgpath))
            {
                richTextBox1.Text = "Please choose your image folder first.";
            }
            else
            {
                GetImageArray(jpgpath);
                if (PicDates.Count() <= 0)
                {
                    richTextBox1.Text = "No jpg images found in " + jpgpath + ".";
                }
                else
                {
                    if (string.IsNullOrEmpty(gpx_logfile) || !gpx_logfile.ToLower().EndsWith(".gpx"))
                    {
                        i = 0;
                        richTextBox1.Text = "Please choose a \".gpx\" file and press \"Geotag Images\" again.";
                    }
                    else
                    {
                        richTextBox1.Text = richTextBox1.Text + "\nProcessing now\n \n";
                        StreamWriter swloctxt = new StreamWriter(jpgpath + Path.DirectorySeparatorChar + "location.txt");
                        Document kml = new Document();
                        StreamWriter swlockml = new StreamWriter(jpgpath + Path.DirectorySeparatorChar + "location.kml");
                        swloctxt.WriteLine("#name longitude/X latitude/Y height/Z yaw pitch roll");

                        string GpxLat = string.Empty;
                        string GpxLon = string.Empty;
                        double GpxCourse = 0;
                        double GpxRoll = 0;
                        double GpxPitch = 0;

                        eTime = ConvertToTimestamp(earliest_picture);

                        XmlTextReader reader = new XmlTextReader(gpx_logfile);
                        while (reader.Read())
                        {
                            if (reader.Name == "trkpt")
                            {
                                reader.MoveToNextAttribute();
                                GpxLat = reader.Value;
                                reader.MoveToNextAttribute();
                                GpxLon = reader.Value.Trim();
                            }
                            if (reader.Name == "ele")
                            {
                                height = reader.ReadElementContentAsString();
                            }
                            if (reader.Name == "time")
                            {
                                isTime = reader.ReadElementContentAsDateTime();
                                Date1 = new DateTime(earliest_picture.Year, earliest_picture.Month, earliest_picture.Day, isTime.Hour, isTime.Minute, isTime.Second);
                               // DateTime localNow = DateTime.Now;
                               // DateTimeOffset localOffset = new DateTimeOffset(DateTime.Now);
                               // Date1 = Date1.Add(new System.TimeSpan(0, 0, 120, 0));
                                Date1 = Date1.ToLocalTime();

                            }

                            if (reader.Name == "course")
                            {
                                GpxCourse = reader.ReadElementContentAsDouble();
                            }
                            if (reader.Name == "roll")
                            {
                                GpxRoll = reader.ReadElementContentAsDouble();
                            }
                            if (reader.Name == "pitch")
                            {
                                GpxPitch = reader.ReadElementContentAsDouble();
                            }

                            if (!isTime.Equals(new DateTime()) && !string.IsNullOrEmpty(GpxLat))
                            {
                                fileList.Add(ConvertToTimestamp(Date1).ToString() + " " + GpxLat + " " + GpxLon + " " + height.ToString() + " " + GpxCourse + " " + GpxRoll + " " + GpxPitch);
                                gpsTimes = fileList.ToArray();
                                GpxLat = string.Empty;
                                GpxLon = string.Empty;
                                isTime = new DateTime();
                            }
                        }


                        foreach (string nline in PicDates)
                        {

                            string[] checktime = nline.Split(' ');
                            string value2 = Array.Find(gpsTimes, element => element.StartsWith(checktime[0], StringComparison.Ordinal));
                            if (string.IsNullOrEmpty(value2))
                            {
                                richTextBox1.Text = richTextBox1.Text + " No coordinates for image " + checktime[1] + " --> " + value2 + "\n";
                            }
                            else
                            {
                                i++;
                                richTextBox1.Text = richTextBox1.Text + " Found coordinates for image " + checktime[1] + " --> " + value2;
                                string[] imgPara = value2.Split(' ');
                                double imgLat = ToDouble(imgPara[1]);
                                double imgLon = ToDouble(imgPara[2]);
                                double imgHei = ToDouble(imgPara[3]);
                                double imgYaw = ToDouble(imgPara[4]);
                                double imgRoll = ToDouble(imgPara[5]);
                                double imgPitch = ToDouble(imgPara[6]);
                                WriteCoordinatesToImage(checktime[1], imgLat, imgLon, imgHei);
                                richTextBox1.Text = richTextBox1.Text + " Done\n";
                                swloctxt.WriteLine(checktime[1] + "\t" + imgPara[2] + "\t" + imgPara[1] + "\t" + imgPara[3] + "\t" + imgPara[4] + "\t" + imgPara[5] + "\t" + imgPara[6]);
                                swloctxt.Flush();

                                SharpKml.Dom.Timestamp tstamp = new SharpKml.Dom.Timestamp();
                                tstamp.When = ConvertFromUnixTimestamp(ToDouble(checktime[0]));
                                kml.AddFeature(
                                    new Placemark()
                                    {
                                        Time = tstamp,
                                        Name = "gtg_" + Path.GetFileName(checktime[1]).ToLower(),
                                        Geometry = new SharpKml.Dom.Point()
                                        {
                                            Coordinate = new Vector(imgLat, imgLon, imgHei)
                                        },
                                        Description = new Description()
                                        {
                                            Text = "<table><tr><td><img src=\"gtg_" + Path.GetFileName(checktime[1]).ToLower() + "\" width=500 /></td></tr></table>"
                                        },
                                        StyleSelector = new Style()
                                        {
                                            Balloon = new BalloonStyle() { Text = "$[name]<br>$[description]" }
                                        }
                                    }
                                );

                                double alpha = 10;
                                RectangleF rect = getboundingbox(imgLat, imgLon, alpha, 100.00, 120.00);
                                kml.AddFeature(
                                         new GroundOverlay()
                                         {
                                             Name = Path.GetFileNameWithoutExtension(checktime[1]),
                                             Visibility = false,
                                             Time = tstamp,
                                             AltitudeMode = AltitudeMode.ClampToGround,
                                             Bounds = new LatLonBox()
                                             {
                                                 Rotation = -alpha % 360,
                                                 North = rect.Bottom,
                                                 East = rect.Right,
                                                 West = rect.Left,
                                                 South = rect.Top,
                                             },
                                             Icon = new SharpKml.Dom.Icon()
                                             {
                                                 Href = new Uri(Path.GetFileName(checktime[1]).ToLower(), UriKind.Relative),
                                             },
                                         }
                                        );
                            }
                        }
                        Uri gpxHref = new Uri(jpgpath, UriKind.Relative);
                        richTextBox1.Text = richTextBox1.Text + i + " of " + PicDates.Count() + " images proceeded\n"+gpxHref;
                        i = 0;
                        swloctxt.Close();
                        Serializer serializer = new Serializer();
                        serializer.Serialize(kml);
                        swlockml.Write(serializer.Xml);
                        swlockml.Close();
                    }
                }
            }
        }       
        RectangleF getboundingbox(double centery, double centerx, double angle, double width, double height)
        {
            double lat = centery;
            double lng = centerx;
            double alpha = angle;

            double ang = degrees(Math.Atan((width / 2.0) / (height / 2.0)));

            double hyplength = Math.Sqrt(Math.Pow(100.00 / 2.0, 2) + Math.Pow(120.00 / 2.0, 2));

            double lat1 = lat;
            double lng1 = lng;

            newpos(ref lat1, ref lng1, alpha + ang, hyplength);

            double lat2 = lat;
            double lng2 = lng;

            newpos(ref lat2, ref lng2, alpha + 180 - ang, hyplength);

            double lat3 = lat;
            double lng3 = lng;

            newpos(ref lat3, ref lng3, alpha + 180 + ang, hyplength);

            double lat4 = lat;
            double lng4 = lng;

            newpos(ref lat4, ref lng4, alpha + 360 - ang, hyplength);

            double minx = 999, miny = 999, maxx = -999, maxy = -999;



            maxx = Math.Max(maxx, lat1);
            maxx = Math.Max(maxx, lat2);
            maxx = Math.Max(maxx, lat3);
            maxx = Math.Max(maxx, lat4);

            minx = Math.Min(minx, lat1);
            minx = Math.Min(minx, lat2);
            minx = Math.Min(minx, lat3);
            minx = Math.Min(minx, lat4);

            miny = Math.Min(miny, lng1);
            miny = Math.Min(miny, lng2);
            miny = Math.Min(miny, lng3);
            miny = Math.Min(miny, lng4);

            maxy = Math.Max(maxy, lng1);
            maxy = Math.Max(maxy, lng2);
            maxy = Math.Max(maxy, lng3);
            maxy = Math.Max(maxy, lng4);

         //   Console.WriteLine("{0} {1} {2} {3}", minx, maxx, miny, maxy);
         //   Console.WriteLine("{0} {1} {2} {3}", lat1, lat2, lat3, lat4);
         //   Console.WriteLine("{0} {1} {2} {3}", lng1, lng2, lng3, lng4);

            return new RectangleF((float)miny, (float)minx, (float)(maxy - miny), (float)(maxx - minx));
        }
        public static double radians(double val)
        {
            return val * deg2rad;
        }
        public static double degrees(double val)
        {
            return val * rad2deg;
        }

        public string[] GetImageArray(string jpgpath)
        {
            
                string[] imageArray = Directory.GetFiles(jpgpath, "*.jpg", SearchOption.TopDirectoryOnly);
                foreach (string jpgName in imageArray)
                {
                    if (jpgName.ToLower().EndsWith(".jpg") && !jpgName.ToLower().Contains("gtg_"))
                    {
                        FileInfo fi1 = new FileInfo(jpgName);
                       richTextBox1.Text = richTextBox1.Text + jpgName + " -> " + fi1.LastWriteTime.ToLongTimeString() + "\n";
                        DateTime t1 = fi1.LastWriteTime;
                        list.Add(t1);
                        picList.Add(ConvertToTimestamp(t1).ToString() + " " + jpgName);
                    }

                }
                list.Sort(delegate(DateTime x, DateTime y) { return y.CompareTo(x); });
                PictureDates = list.ToArray();
                PicDates = picList.ToArray();
                picList.Clear();
                list.Clear();
                foreach (DateTime ePicTime in PictureDates)
                {
                    earliest_picture = ePicTime;
                }
                richTextBox1.Text = richTextBox1.Text + PicDates.Count() + " images found \n";
            
        
            
            return PicDates;
        }

        void newpos(ref double lat, ref double lon, double bearing, double distance)
        {
            // '''extrapolate latitude/longitude given a heading and distance
            // thanks to http://www.movable-type.co.uk/scripts/latlong.html
            // '''
            // from math import sin, asin, cos, atan2, radians, degrees
            double radius_of_earth = 6378100.0;//# in meters

            double lat1 = radians(lat);
            double lon1 = radians(lon);
            double brng = radians((bearing + 360) % 360);
            double dr = distance / radius_of_earth;

            double lat2 = Math.Asin(Math.Sin(lat1) * Math.Cos(dr) +
                        Math.Cos(lat1) * Math.Sin(dr) * Math.Cos(brng));
            double lon2 = lon1 + Math.Atan2(Math.Sin(brng) * Math.Sin(dr) * Math.Cos(lat1),
                                Math.Cos(dr) - Math.Sin(lat1) * Math.Sin(lat2));

            lat = degrees(lat2);
            lon = degrees(lon2);
            //return (degrees(lat2), degrees(lon2));
        }

        
    }
}